#include <QPainter>
#include <QDebug>

#include <cmath>

#include "QiLaserScan.h"

QiLaserScan::QiLaserScan( LaserData const& lms ) : m_lms( lms ), m_scale( 50.0 )
{
}

void QiLaserScan::setScale( const double scale )
{
    m_scale = scale;
    update();
}

void QiLaserScan::paint( QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget )
{
    if ( m_lms.NUM_SHOTS <= 0)
	return;

    QPen pen;

    pen.setWidth( 2 );
    pen.setColor( Qt::red );
    painter->setPen( pen );

    painter->drawEllipse( QPoint(0, 0), 10, 10 );
    painter->drawLine( QPoint(0, 0), QPoint(0, 10) );

    pen.setWidth( 3 );
    pen.setColor( Qt::blue );
    painter->setPen( pen );
    /*
    double scale, angle, d, x, y;

    scale = m_scale;
    angle = -120.0 * M_PI/180.0;

    d = 5;
    x = scale * d * cos( M_PI/2.0 + angle );
    y = scale * d * sin( M_PI/2.0 + angle );

    painter->drawPoint( QPointF( x, y ) );

    angle = +120.0 * M_PI/180.0;

    d = 5;
    x = scale * d * cos( M_PI/2.0 + angle );
    y = scale * d * sin( M_PI/2.0 + angle );

    painter->drawPoint( QPointF( x, y ) );

    angle = +0.0 * M_PI/180.0;

    d = 2;
    x = scale * d * cos( M_PI/2.0 + angle );
    y = scale * d * sin( M_PI/2.0 + angle );

    painter->drawPoint( QPointF( x, y ) );

    */

    //setScale( 10.0 );


    double angle = m_lms.START_ANGLE;
    double x, y;

    for ( int i = 0; i < m_lms.NUM_SHOTS; i++ )
    {
	if ( m_lms.dist[i] > 0 )
	{
	    x = m_scale * m_lms.dist[i] * cos( M_PI/2.0 + angle );
	    y = m_scale * m_lms.dist[i] * sin( M_PI/2.0 + angle );

	    painter->drawPoint( QPointF( x, y ) );
	}

	angle += m_lms.RESOLUTION;
    }

}

QRectF QiLaserScan::boundingRect() const
{
    return QRectF( -m_scale * m_lms.MAX_RANGE, -m_scale * m_lms.MAX_RANGE,
		   2.0 * m_scale * m_lms.MAX_RANGE, 2.0 * m_scale * m_lms.MAX_RANGE );
}

void QiLaserScan::keyPressEvent( QKeyEvent * event )
{
    switch (event->key())
    {
	case Qt::Key_Plus:
	    setScale( m_scale * 1.2 );
	    break;
	case Qt::Key_Minus:
	    m_scale *= 1.0 / 1.2;
	    break;
    }
}

void QiLaserScan::wheelEvent(QWheelEvent *event)
{
    //scaleView(pow(2., event->delta() / 240.0));
    m_scale *= pow( 2., event->delta() / 240.0 );
}
